#include <asio_driver_special/motor_dc_io.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <general_interface/action/switch_connection.hpp>

class ConnectionControlNode : public rclcpp::Node
{
  private:
    AD::AsioContext::SharePtr context;
    std::unique_ptr<AD::Special::MotorDC_Mode1> motorLeft;
    std::unique_ptr<AD::Special::MotorDC_Mode1> motorRight;
    rclcpp_action::Server<general_interface::action::SwitchConnection>::SharedPtr connectAction;
    rclcpp::TimerBase::SharedPtr timer;
    std::shared_ptr<rclcpp_action::ServerGoalHandle<general_interface::action::SwitchConnection>> handle;
    unsigned char state = 0;

  public:
    ConnectionControlNode() : Node("connection_node")
    {
        context = AD::AsioContext::CreatAsioContext();
        motorLeft = std::make_unique<AD::Special::MotorDC_Mode1>(context);
        motorRight = std::make_unique<AD::Special::MotorDC_Mode1>(context);

        bool success = false;

        success = motorLeft->Open(AD::Special::MotorDC_Mode1::Config{
            .pinDirection = 63,
            .pinBrake = 62,
            .pinFeedback = 101,
            .pinPWM = 47,
        });
        if (success)
        {
            RCLCPP_INFO(this->get_logger(), "Motor Left Init successfully");
            motorLeft->SetBrake(false);
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Motor Left Init failed");
            return;
        }

        success = motorRight->Open(AD::Special::MotorDC_Mode1::Config{
            .pinDirection = 113,
            .pinBrake = 46,
            .pinFeedback = 35,
            .pinPWM = 114,
        });
        if (success)
        {
            RCLCPP_INFO(this->get_logger(), "Motor Right Init successfully");
            motorRight->SetBrake(false);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Motor Right Init failed");
            return;
        }
        RCLCPP_INFO(get_logger(), "Hardware is finished initializing");

        RCLCPP_INFO(get_logger(), "Start init ROS2 related stuff");
        connectAction = rclcpp_action::create_server<general_interface::action::SwitchConnection>(
            this, "switch_connection",
            std::bind(&ConnectionControlNode::on_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&ConnectionControlNode::on_cancel, this, std::placeholders::_1),
            std::bind(&ConnectionControlNode::on_accept, this, std::placeholders::_1));
        timer = this->create_timer(std::chrono::milliseconds(100), std::bind(&ConnectionControlNode::on_tick, this));
    }

  private:
    rclcpp_action::GoalResponse on_goal(const rclcpp_action::GoalUUID &uuid,
                                        std::shared_ptr<const general_interface::action::SwitchConnection::Goal> goal)
    {
        (void)uuid;
        RCLCPP_INFO(get_logger(), "Received goal %d", goal->command);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse on_cancel(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<general_interface::action::SwitchConnection>> goalHandle)
    {
        (void)goalHandle;
        if (state == general_interface::action::SwitchConnection::Result::CONNECTING ||
            state == general_interface::action::SwitchConnection::Result::DISCONNECTING)
        {
            RCLCPP_INFO(get_logger(), "Canceling action,request by client");
            state = general_interface::action::SwitchConnection::Result::PAUSED;
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        }
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void on_accept(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<general_interface::action::SwitchConnection>> goalHandle)
    {
        if (this->handle != nullptr)
        {
            RCLCPP_WARN(get_logger(), "Received new goal while previous goal is still executing");
            auto result = std::make_shared<general_interface::action::SwitchConnection::Result>();
            result->success = false;
            result->result = state;
            handle->abort(result);
            handle = nullptr;
        }

        auto goal = goalHandle->get_goal();
        if (goal->command == general_interface::action::SwitchConnection::Goal::STOP)
        {
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            if (state == general_interface::action::SwitchConnection::Result::CONNECTING ||
                state == general_interface::action::SwitchConnection::Result::DISCONNECTING)
                state = general_interface::action::SwitchConnection::Result::PAUSED;

            auto result = std::make_shared<general_interface::action::SwitchConnection::Result>();
            result->success = true;
            result->result = state;
            goalHandle->succeed(result);
            return;
        }
        else if (goal->command == general_interface::action::SwitchConnection::Goal::CONNECT)
        {
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Clockwise);
            state = general_interface::action::SwitchConnection::Result::CONNECTING;
        }
        else if (goal->command == general_interface::action::SwitchConnection::Goal::DISCONNECT)
        {
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::CounterClockwise);
            state = general_interface::action::SwitchConnection::Result::DISCONNECTING;
        }
        // durat = goal->timeout;
        handle = goalHandle;
    }

    void on_tick()
    {
        if (state != general_interface::action::SwitchConnection::Result::CONNECTING &&
            state != general_interface::action::SwitchConnection::Result::DISCONNECTING)
            return;
        context->SpinOnce();
        int tick = motorLeft->GetFeedback() + motorRight->GetFeedback();
        if (tick > 0)
        {
            auto feedback = std::make_shared<general_interface::action::SwitchConnection::Feedback>();
            feedback->current = state;
            handle->publish_feedback(feedback);
            return;
        }

        if (state == general_interface::action::SwitchConnection::Result::CONNECTING)
        {
            state = general_interface::action::SwitchConnection::Result::CONNECTED;
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        }
        else if (state == general_interface::action::SwitchConnection::Result::DISCONNECTING)
        {
            state = general_interface::action::SwitchConnection::Result::DISCONNECTED;
            motorLeft->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
            motorRight->SetDirection(AD::Special::MotorDC_Mode1::Direction::Stop);
        }
        else
        {
            RCLCPP_ERROR(get_logger(), "Unknown state: %d", state);
            return;
        }
        auto result = std::make_shared<general_interface::action::SwitchConnection::Result>();
        result->success = true;
        result->result = state;
        handle->succeed(result);
        handle = nullptr;
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ConnectionControlNode>());
    rclcpp::shutdown();
    return 0;
}